#include "init.h"
#include "statechanges.h"

#define _DEFAULT_PWM_STATE _IOCON1_PENH_MASK | _IOCON1_PENL_MASK  //| _IOCON1_PMOD0_MASK | _IOCON1_PMOD1_MASK

/** Openloop BEMF: (+ ==> complementary output, - ==> fixed low)
 *          A   B   C
 * Step 0   x   -   +
 * Step 1   +   -   x
 * Step 2   +   x   -
 * Step 3   x   +   -
 * Step 4   -   +   x
 * Step 5   -   x   +
 */

const unsigned int PWM_CON_STATES[] = {
    _DEFAULT_PWM_STATE | _IOCON1_OVRENH_MASK | _IOCON1_OVRENL_MASK, // 1
    _DEFAULT_PWM_STATE,                        // 2
    _DEFAULT_PWM_STATE,                        // 3
    _DEFAULT_PWM_STATE | _IOCON1_OVRENH_MASK | _IOCON1_OVRENL_MASK, // 4
    _DEFAULT_PWM_STATE | _IOCON1_OVRENH_MASK | _IOCON1_OVRENL_MASK | _IOCON1_OVRDAT0_MASK, // 5
    _DEFAULT_PWM_STATE | _IOCON1_OVRENH_MASK | _IOCON1_OVRENL_MASK | _IOCON1_OVRDAT0_MASK, // 6
    _DEFAULT_PWM_STATE | _IOCON1_OVRENH_MASK | _IOCON1_OVRENL_MASK, // 1
    _DEFAULT_PWM_STATE,                        // 2
    _DEFAULT_PWM_STATE,                        // 3
    _DEFAULT_PWM_STATE | _IOCON1_OVRENH_MASK | _IOCON1_OVRENL_MASK // 4
};

void __attribute__((interrupt, no_auto_psv)) _T3Interrupt(void)
{
    T3ISR_ENTER;

    extern struct state_t state;
    extern struct pid_t pid;
    extern struct setup_t setup;
    IFS0bits.T3IF = 0;    //GLED = 1;

    if (state.mode == STATE_OFF)
    {
        T3ISR_EXIT;
        return;
    }

/*
    if (PR3 < 65535 / 32 && state.RPMpulseDiv == 8)
    {
        state.RPMpulseDiv = 1;
        T3CONbits.TCKPS = 0;
    }
    else if (PR3 > 65535 / 2 && state.RPMpulseDiv == 1)
    {
        state.RPMpulseDiv = 8;
        T3CONbits.TCKPS = 1;
    }*/

    if (state.zc_found == 0)
    {
        state.errorcount++;
        if (state.errorcount > setup.errortreshold || state.mode == STATE_STOPPING)
        {
            stateOffEnter();
            T3ISR_EXIT;
            return;
        }
    }
    else
    {
        state.errorcount = 0;
    }

    /*
    if (state.mode == STATE_BEMF_SAMPLING && state.ignore_zc == 0)
    {
        if (state.zc_found == 0)
        {
            if (state.errorcount>setup.errortreshold)
            {
                MDC = 0;
                stateOffEnter();
            }
            else
            {
                state.errorcount++;
            }
        }
        else
        {
            state.errorcount = 0;
        }
        return;
    }*/
    if (state.mode == STATE_BEMF_SAMPLING && state.rampup >= setup.OPENLOOP_ZC)
    {
        asm("NOP");
        T3ISR_EXIT;
        return;
    }

    if (state.mode == STATE_ALIGN)
    {
        if (MDC < setup.OPENLOOP_POWER_START)
        {
            MDC += 50; //setup.OPENLOOP_POWER_START / 20;
            T3ISR_EXIT;
            return;
        }
        else
        {
            pid.i_value = setup.OPENLOOP_POWER_START;
            pid.output = pid.i_value;
            state.rampup = 0;
            stateBemfCommutationEnter();
        }
    }

// commute!
    state.step++;
    if (state.step==6)
    {
        state.step=0;
    }

    if (state.direction == 0)
    {
        IOCON1 = PWM_CON_STATES[state.step];// | _IOCON1_OVRENL_MASK; // no complementary!
        IOCON2 = PWM_CON_STATES[state.step-2+6];// | _IOCON1_OVRENL_MASK;
        IOCON3 = PWM_CON_STATES[state.step-4+6];// | _IOCON1_OVRENL_MASK;


        FCLCON1bits.CLMOD = (state.step == 1 || state.step == 2) ? 1 : 0;
        FCLCON2bits.CLMOD = (state.step == 3 || state.step == 4) ? 1 : 0;
        FCLCON3bits.CLMOD = (state.step == 5 || state.step == 0) ? 1 : 0;
    }
    else
    {
        IOCON1 = PWM_CON_STATES[state.step];// | _IOCON1_OVRENL_MASK; // no complementary!
        IOCON3 = PWM_CON_STATES[state.step-2+6];// | _IOCON1_OVRENL_MASK;
        IOCON2 = PWM_CON_STATES[state.step-4+6];// | _IOCON1_OVRENL_MASK;


        FCLCON1bits.CLMOD = (state.step == 1 || state.step == 2) ? 1 : 0;
        FCLCON3bits.CLMOD = (state.step == 3 || state.step == 4) ? 1 : 0;
        FCLCON2bits.CLMOD = (state.step == 5 || state.step == 0) ? 1 : 0;
    }

    state.DEMAG=state.RPMpulse / 256;

    if (state.DEMAG<=2)
    {
        state.DEMAG=2;
    }
    else
    {
        if (state.DEMAG>=10)
        {
            state.DEMAG=10;
        }
    }
    state.DEMAG = 100;

    {
        stateBemfSamplingEnter();
        if (state.rampup < setup.OPENLOOP_ZC)
        {
            state.rampup++;
            PR3 = setup.OPENLOOP_SPEED_START;
        }
        else
        {
            PR3 = 65500;
        }
        
    }
    T3ISR_EXIT;
}
